#include "serial.h"
#include "json.hpp"

using json = nlohmann::json;

using namespace LibSerial;

std::string serial_dev = "/dev/ttyUSB0";


// float kp_x = 30.0f;
// float kp_y = -1500.0f;
// float offest = 25;

serial_ctl::serial_ctl(const std::string& port_name): port_name_(port_name)
{
    get_protocol_version();
    serial_port_ = std::make_shared<SerialPort>(port_name);
    serial_init();
    std::cout << " open serial port " << port_name_ << std::endl;
}

serial_ctl::~serial_ctl()
{
    serial_close();
    serial_port_ = nullptr;
    std::cout << " release serial port " << port_name_ << std::endl;
}

void serial_ctl::serial_init(void)
{
    serial_port_->Open( );
    serial_port_->SetBaudRate( SerialPort::BAUD_460800 );
    serial_port_->SetCharSize( SerialPort::CHAR_SIZE_8 );
    serial_port_->SetNumOfStopBits( SerialPort::STOP_BITS_1 ) ;
    
    if(protocol_version_ == 1)
    {
        // enable device
        std::vector<unsigned char> databuffer(14);
        databuffer.at(0) =  0x55;
        databuffer.at(1) =  0x0e;
        databuffer.at(2) =  0x00;
        databuffer.at(3) =  0x31;
        // body
        databuffer.at(4) =  0x01;//(Vx>>8)& 0xff;
        databuffer.at(5) =  0x01;//Vx & 0xff;
        databuffer.at(6) =  0x04; 
        databuffer.at(7) =  0x01;
        //reserve
        databuffer.at(8) =  0x01;
        databuffer.at(9) =  0x00;
        databuffer.at(10) =  0x00;
        databuffer.at(11) =  0x00;
        // end
        databuffer.at(12) =  0x01;
        uint32_t sum = 0;
        for(size_t i = 0; i < 13; i++)
            sum +=  databuffer.at(i);
        databuffer.at(13) =  sum & 0xff;
        
        serial_port_->Write( databuffer);
    }
    
    std::vector<unsigned char> databuffer(14);
    databuffer.at(0) =  0x55;
    databuffer.at(1) =  0x0e;
    databuffer.at(2) =  0x00;
    databuffer.at(3) =  0x21;
    if(protocol_version_ == 1)
    {
        databuffer.at(2) =  0x01;
        databuffer.at(3) =  0x12;
    }
        
    // body
    databuffer.at(4) =  0x02;//(Vx>>8)& 0xff;
    databuffer.at(5) =  0x00;//Vx & 0xff;
    databuffer.at(6) =  0x00; 
    databuffer.at(7) =  0x00;
    //reserve
    databuffer.at(8) =  0x00;
    databuffer.at(9) =  0x00;
    databuffer.at(10) =  0x00;
    databuffer.at(11) =  0x00;
    // end
    databuffer.at(12) =  0x01;
    uint32_t sum = 0;
    for(size_t i = 0; i < 13; i++)
        sum +=  databuffer.at(i);
    databuffer.at(13) =  sum & 0xff;
    
    serial_port_->Write( databuffer);

    // for(size_t i =0; i < 3; i++)
        // send_reset();

}

void serial_ctl::serial_close(void)
{
    for(size_t i =0; i < 3; i++)
    {
        send_reset();
    }
    std::cout << " send reset cmd " << std::endl;
    serial_port_->Close();
}

void serial_ctl::send_cmd(float forward, float yaw)
{
    // float dy = atan2(yaw , forward);
    int16_t Vx=0,Vy=0;
    Vx = forward;//(forward - offest)* kp_x;
    Vy = yaw;//dy* kp_y;
    // const int BUFFER_SIZE = 20;
    // char output_buffer[50];
    std::vector<unsigned char> databuffer(14);
    databuffer.at(0) =  0x55;
    databuffer.at(1) =  0x0e;
    databuffer.at(2) =  0x01;
    databuffer.at(3) =  0x11;
    if(protocol_version_ == 1)
        databuffer.at(3) =  0x13;
    
    // body
    databuffer.at(4) =  (Vx>>8)& 0xff;
    databuffer.at(5) =  Vx & 0xff;
    databuffer.at(6) =  0x00;
    databuffer.at(7) =  0x00;
    databuffer.at(8) =  (Vy>>8)& 0xff;
    databuffer.at(9) =  Vy & 0xff;
    //reserve
    databuffer.at(10) =  0x00;
    databuffer.at(11) =  0x00;
    // end
    databuffer.at(12) =  0x01;
    uint32_t sum = 0;
    for(size_t i = 0; i < 13; i++)
        sum +=  databuffer.at(i);
    databuffer.at(13) =  sum & 0xff;
    
    serial_port_->Write( databuffer);
    // std::cout << " send cmd "<<Vy << std::endl;
}

void serial_ctl::send_reset(void)
{
    // const int BUFFER_SIZE = 20;
    // char output_buffer[50];
    std::vector<unsigned char> databuffer(14);
    databuffer.at(0) =  0x55;
    databuffer.at(1) =  0x0e;
    databuffer.at(2) =  0x01;
    databuffer.at(3) =  0x11;
    if(protocol_version_ == 1)
        databuffer.at(3) =  0x13;
    // body
    databuffer.at(4) =  0x00;//(Vx>>8)& 0xff;
    databuffer.at(5) =  0x00;//Vx & 0xff;
    databuffer.at(6) =  0x00; 
    databuffer.at(7) =  0x00;
    databuffer.at(8) =  0x00;
    databuffer.at(9) =  0x00;
    //reserve
    databuffer.at(10) =  0x00;
    databuffer.at(11) =  0x00;
    // end
    databuffer.at(12) =  0x01;
    uint32_t sum = 0;
    for(size_t i = 0; i < 13; i++)
        sum +=  databuffer.at(i);
    databuffer.at(13) =  sum & 0xff;
    


    serial_port_->Write( databuffer);
    
}

void serial_ctl::get_protocol_version(void)
{
    std::ifstream file("../parameters.json");
    json j;
    file >> j; 
    protocol_version_ = j["protocol_version"];
}